import os
from os.path import join
import sys
sys.path.append("..")
import numpy as np
import copy
from utils.bvh2joint import bvh2joint, default_end_link_trans
from utils.fbx2objpose import fbx2objpose
import open3d as o3d


if __name__ == "__main__":
    human1_path = "../exp_data/20230407_data/mocap+rgbd_person1.bvh"
    obj1_path = "../exp_data/20230407_data/mocap+rgbd_chair1.fbx"
    objmodel_path = "../exp_data/objects/20230407_chair1/chair_m.obj"

    joint_data = bvh2joint(human1_path, frame_ids=None, end_link_trans=default_end_link_trans())
    objpose = fbx2objpose(obj1_path)
    objmodel = o3d.io.read_triangle_mesh(objmodel_path)

    assert joint_data.shape[0] == objpose.shape[0]
    N = joint_data.shape[0]
    
    idx = 4378
    # visualize human joint
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(joint_data[idx])
    o3d.io.write_point_cloud("./vis_joint.ply", pcd)

    # visualize object
    pose = objpose[idx]
    mesh = copy.deepcopy(objmodel)
    mesh = mesh.transform(pose)
    o3d.io.write_triangle_mesh("./vis_obj.obj", mesh)
